#include "MathHelper.h"
#include "math.h"
inline int16_t linear_map(int32_t input, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) {
    // 处理输入范围无效的情况
    if (in_min == in_max) {
        return out_min; // 返回默认值，可选择out_min或其它处理方式
    }
    if(input < in_min) {
        return out_min;
    }
    if(input > in_max) {
        return out_max;
    }
    // 处理输出范围无效的情况
    if (out_min == out_max) {
        return out_min; // 返回默认值，可选择out_min或其它处理方式
    }
    
    // 转换为64位类型防止中间结果溢出
    int64_t input64 = (int64_t)input;
    int64_t in_min64 = (int64_t)in_min;
    int64_t in_max64 = (int64_t)in_max;
    int64_t out_min64 = (int64_t)out_min;
    int64_t out_max64 = (int64_t)out_max;
    
    // 计算范围和比例
    int64_t in_range = in_max64 - in_min64;
    int64_t out_range = out_max64 - out_min64;
    int64_t scaled = (input64 - in_min64) * out_range / in_range;
    
    // 计算最终结果并钳位到int16_t范围
    int64_t result = scaled + out_min64;
    
    if (result > INT16_MAX) {
        return INT16_MAX;
    } else if (result < INT16_MIN) {
        return INT16_MIN;
    } else {
        return (int16_t)result;
    }
}


/**
 * @brief 实现带有限幅功能的线性映射
 * @param input_value 原始输入值
 * @param input_min 输入范围最小值
 * @param input_max 输入范围最大值
 * @param output_min 输出范围最小值
 * @param output_max 输出范围最大值
 * @return 映射并限幅后的输出值 (浮点数)
 *
 * @note 自动处理以下场景：
 * - 输入范围反向 (input_max < input_min)
 * - 输出范围反向 (output_max < output_min)
 * - 输入范围为零宽 (input_max == input_min)
 */
inline float linear_map_f(
    float input_value,
    float input_min,
    float input_max,
    float output_min,
    float output_max
) {
    /* 处理输入范围为零的情况 */
    if (input_max == input_min) {
        return (output_min + output_max) / 2.0f; // 返回输出范围中点
    }

    /* 计算标准化值 */
    float normalized = (input_value - input_min) / (input_max - input_min);
    
    /* 线性映射 */
    float output_value = normalized * (output_max - output_min) + output_min;
    
    /* 智能限幅处理 */
    float lower_bound = fminf(output_min, output_max);
    float upper_bound = fmaxf(output_min, output_max);
    
    return output_value < lower_bound ? lower_bound : 
           output_value > upper_bound ? upper_bound : output_value;
}